/**
 * Arduino两轮小车
 * 
 * 本程序可不受限制的用于学习和教学，商业用途请联系作者。
 * 本程序不对用户因使用程序而造成任何形式的损失负责。
 * 
 * site: http://www.xpstem.com/maker/arduino-vehicle
 * author: billy zhang
 */
 
#include <Arduino.h>
#include "TwoWheelDriven.h"

#define TurnMaxPower 180
#define NoPower 0

TwoWheelDriven::TwoWheelDriven(uint8_t leftMotorDir, uint8_t leftMotorEn, uint8_t rightMotorDir, uint8_t rightMotorEn)
{
  leftMotorDir_ = leftMotorDir;
  leftMotorEn_ = leftMotorEn;
  rightMotorDir_ = rightMotorDir;
  rightMotorEn_ = rightMotorEn;
  
  pinMode(leftMotorDir_, OUTPUT);
  pinMode(leftMotorEn_, OUTPUT); 
  pinMode(rightMotorDir_, OUTPUT);
  pinMode(rightMotorEn_, OUTPUT); 
}

void TwoWheelDriven::drive(bool leftDir, uint8_t leftPower, bool rightDir, uint8_t rightPower) {
  digitalWrite(leftMotorDir_, leftDir);
  digitalWrite(rightMotorDir_, rightDir);
  analogWrite(leftMotorEn_, leftPower);
  analogWrite(rightMotorEn_, rightPower);
}

void TwoWheelDriven::stop()
{
  digitalWrite(leftMotorDir_, LOW);
  digitalWrite(rightMotorDir_, LOW);
  analogWrite(leftMotorEn_, 0);
  analogWrite(rightMotorEn_, 0);
}

void TwoWheelDriven::forward(uint8_t power)
{
  drive(LOW, power, LOW, power);
}

void TwoWheelDriven::backward(uint8_t power)
{
   drive(HIGH, power, HIGH, power);
}

void TwoWheelDriven::turnLeft(uint8_t power)
{
  drive(LOW, power, HIGH, power);
}

void TwoWheelDriven::turnRight(uint8_t power)
{
  drive(HIGH, power, LOW, power);
}

TwoWheelDriven::~TwoWheelDriven()
{
}
